perm filename SUBR.PAL[V,VDS]1 blob
sn#264830 filedate 1977-02-15 generic text, type C, neo UTF8
COMMENT ⊗ VALID 00016 PAGES
C REC PAGE DESCRIPTION
C00001 00001
C00003 00002 .TITLE SUBR
C00007 00003 "GETSYM" - FETCHES THE DESCRIPTOR BLOCK FOR A GIVEN STRING NAME
C00011 00004 "GETTRN"&"GETPRG" - DECODES NAME INTO POINTER TO SYMBOL BLOCK
C00014 00005 "GETSTR" - SUBR. TO SAVE STRING POINTER AND ADVANCE SG REGISTER
C00015 00006 "GETBLK" - FREE STORAGE ALLOCATOR
C00019 00007 "RELBLK" - RETURNS FREE STORAGE BLOCK
C00021 00008 "TYPERR" - TYPES OUT ERROR MESSAGES
C00023 00009 ERROR CODE BITS
C00027 00010 "PACNME" - SUBR. TO PACK A SYMBOLIC NAME INTO A OUTPUT BUFFER
C00029 00011 "PTRTRN" - SUBR. TO PRINT A TRANSFORMS NAME AND DATA
C00031 00012 "PTRANS" - SUBR. TO PRINT A TRANSFORMS X,Y,Z,O,A,T
C00033 00013 "PSTEP" - SUBR. TO PRINT MOTION INSTRUCTION OUT ON TTY
C00036 00014 "MODTRN" - SUBR. TO PERMIT MODIFICATION OF EXISTING TRANSFORMS
C00039 00015 "EVAL" - EVALUATES A 5TH ORDER POLYNOMIAL
C00042 00016 "TIMER" - COMPUTE TOTAL MOTION TIME
C00045 ENDMK
C⊗;
.TITLE SUBR
;"PUSARG" - DECODES A FUNCTION AND ITS ARGUMENTS
;THIS ROUTINES DECODES A STRING FUNCTION NAME AND LOCATES ITS SYMBOL
;DATA BLOCK. THE ARGUMENTS OF THE FUNCTION ARE THEN DECODED AND LEFT
;ON THE STACK. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #HASTAB,R0 ;PTR TO SYMBOL HASH TABLE
; MOV #TYPE,R1 ;TYPE OF FUNCTION TO DECODE
; JSR PC,PUSARG
; BCS ERROR ;SET IF ERROR OCCURS
;
;IF NO ERROR OCCURS, R0 ← PTR TO SYMBOL DATA BLOCK AND A BLOCK OF
;EIGHT WORDS ARE LEFT ON THE STACK. THE WORDS ON THE STACK ARE USED
;TO STORE THE FUNCTION ARGUMENTS THAT ARE DECODED. THE FIRST
;ARGUEMENT HAS THE LOWEST CORE ADDRESS. IF AN ERROR OCCURS, THE C
;BIT IS SET, THE STACK IS LEFT UNALTERED AND R1 IS USED TO INDICATE
;THE TYPE OF ERROR:
;
; R1 = 0, NO SYMBOLIC FUNCTION NAME FOUND
; R1 ≠ 0, ERROR MESSAGES IN R1
;REGISTERS USED:
; ALL REGISTERS ARE ALTERED
PUSARG: JSR PC,GETSYM ;GET THE FUNCTION SYMBOL DATA BLK
BCC GOTFUN
MOV R1,R1 ;CHECK ERROR CODE
BPL .+6
MOV #UNKFUN,R1 ;EXIT IMMEDIATELY IF NO SYMBOL FOUND
RTS PC
GOTFUN: SUB #20,SP ;LEAVE ROOM ON STACK FOR ARGUMENTS
MOV 20(SP),(SP) ;SAVE RETURN ADDRESS
MOV R0,-(SP) ;SAVE PTR TO SYMBOL DATA BLOCK
MOV SP,R4 ;PTR TO ARGUMENT STORAGE
CMP (R4)+,(R4)+
MOV FUNARG(R0),R3 ;ARGUMENT TYPE INDICATORS
BEQ PUSDNE ;ALL DONE IF NO ARGUMENTS
MOV FUNARG+2(R0),R2
BR .+6
GETARG: BIC #170000,R2 ;DONT WANT SIGN BIT EXTENDED
MOV R3,R0 ;NEXT ARGUMENT TYPE
BIC #177761,R0
JSR PC,@ARGTAB(R0) ;GO DECODE ARGUMENT
BCC GOTARG
MOV R1,R1 ;BRANCH IF SYNTAX ERROR
BNE ARGERR
BIT #1,R3 ;ARG MISSING, ERROR IF NOT OPTIONAL
BEQ NOARG
CLR R0 ;DEFAULT = 0
GOTARG: MOV R0,(R4)+ ;SAVE ARGUMENT VALUE
JSR PC,CLRCMA
BCS ARGERR
ASHC #-4,R2 ;REPEAT FOR ALL ARGUMENTS
BNE GETARG
PUSDNE: CLC ;NO ERROR
MOV (SP)+,R0 ;PTR TO SYMBOL DATA BLOCK
RTS PC
NOARG: MOV #NOARGU,R1 ;INDICATE NO ARGUMENT FOUND
ARGERR: MOV 2(SP),R0 ;THIS IS THE RETURN ADDRESS
ADD #24,SP ;CLEAR STACK
SEC ;INDICATE ERROR
JMP (R0)
;END OF "PUSARG"
;"GETSYM" - FETCHES THE DESCRIPTOR BLOCK FOR A GIVEN STRING NAME
;THE FIRST WORD IN THE STRING POINTER BUFFER IS HASHED AND A SEARCH
;OF THE APPROPRIATE HASH BUCKET IS CONDUCTED. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #HASHTB,R0 ;PTR TO HASH TABLE
; MOV #TYPE,R1 ;NAME ID, EG. MOTION, MASTER
; MOV #STRING,SG ;STRING CONTAINING NAME
; JSR PC,GETSYM
; BCS ERROR ;SET IF ERROR
;
;IF SUCCESSFUL, R0 ← PTR TO SYMBOL DATA BLOCK AND SG IS LEFT
;POINTING AT THE BREAK CHARACTER. IF AN ERROR OCCURRED, THE C
;BIT IS SET AND R1 INDICATES THE TYPE OF ERROR:
;
; R1 = 0, NO SYMBOLIC NAME FOUND
; R1 > 0, TOO MANY CHARACTERS IN NAME, R1= ERROR CODE
; R1 < 0, NO MATCH FOR NAME FOUND, R0 ← PTR TO LAST DATA BLK
; IN HASH BUCKET, R1 ← -# OF CHAR IN NAME, SG ← PTR TO
; FIRST CHARACTER IN NAME.
;REGISTERS USED:
; R0,R1,SG PASS ARGUMENTS AND MAY BE ALTERED
GETSYM: MOV R4,-(SP) ;SAVE REGISTERS
MOV R3,-(SP)
MOV R2,-(SP)
MOV R1,-(SP) ;SAVE SYMBOL TYPE
;HASH THE FIRST WORD
CMPB #40,(SG)+ ;IGNOR ALL LEADING SPACE CHARACTERS
BEQ .-4
DEC SG ;POINT TO FIRST NON-SPACE CHARACTER
MOV SG,R4 ;SAVE STRING POINTER
MOV #7,R1 ;HASH AT MOST 6 CHARACTERS
CLR R2 ;FORM HASH IN HERE
HASH1: CMPB #15,(SG) ;CHECK IF END OF LINE = CR CHARACTER
BEQ HASH2
CMPB #40,(SG) ;CHECK IF END OF WORD = SPACE CHAR
BEQ HASH2
CMPB #54,(SG) ;COMMAS ALSO SEPARATE WORDS
BEQ HASH2
MOVB (SG)+,R3
ADD R3,R2 ;ELSE ADD CHARACTERS TOGETHER
SOB R1,HASH1 ;CHECK IF MORE THAN 6 CHAR. READ
MOV #BIGSYM,R1 ;INDICATE TOO MANY CHARACTERS IN WORD
BR GTSERR
HASH2: SUB #7,R1 ;CHECK IF ANY CHARACTERS FOUND
BEQ GTSERR ;EXIT IF NO WORD BEFORE BREAK CHAR.
BIC #177740,R2 ;USE 5 LSB AS HASH WORD INDEX
ASL R2
ADD R2,R0 ;ADD TO BASE ADDRESS OF TABLE
;GO SEARCH FOR SYMBOL
GETSM1: MOV R4,SG ;POINT TO START OF SYMBOL
TST (R0) ;TEST IF ANY MORE SYMBOLS IN BUCKET
BEQ GTSERR ;EXIT IF DIDN'T FIND A MATCH
MOV (R0),R0 ;PTR TO NEXT SYMBOL BLOCK
BIT (SP),TYPBIT(R0) ;SAME TYPE OF SYMBOL?
BEQ GETSM1
MOV R0,R3 ;COMPARE NAME
ADD #SYMNME,R3
MOV R1,R2
NEG R2
GETSM2: CMPB (R3)+,(SG)+
BNE GETSM1 ;BRANCH IF NOT SAME
SOB R2,GETSM2
CMP #-6,R1 ;PERFECT MATCH IF 6 CHARACTERS
BEQ GTSDNE
CMPB (R3),#40 ;ELSE THIS BETTER BE A SHORT SYM.
BEQ GTSDNE
BR GETSM1
GTSERR: SEC ;INDICATE ERROR
GTSDNE: MOV (SP),(SP)+ ;DISCARD TYPE WORD
MOV (SP)+,R2 ;RESTORE REGISTERS
MOV (SP)+,R3
MOV (SP)+,R4
RTS PC
;END OF "GETSYM"
;"GETTRN"&"GETPRG" - DECODES NAME INTO POINTER TO SYMBOL BLOCK
;THESE TWO ROUTINES DECODE THE NAMES OF PROGRAMS AND TRANSFORMS INTO
;POINTERS TO DATA SYMBOL BLOCKS. A SAMPLE CALL TO ONE OF THESE
;ROUTINES FOLLOWS:
;
; MOV #STRING,SG ;POINT TO INPUT STRING
; JSR PC,GETTRN ;NO ARGUMENTS REQUIRED
; BCS ERROR ;CHECK FOR ERROR RETURN
;
;IF A SYMBOLIC NAME IS FOUND A SYMBOL BLOCK IS ALLOCATED IF THE
;NAME IS NOT ALREADY DEFINED. IN EITHER CASE, THE C BIT IS LEFT
;CLEARED AND R0 ← PTR TO SYMBOL BLOCK. IF NO SYMBOLIC NAME IS
;FOUND, C IS SET AND R1← 0, OTHERWISE C SET AND R1 ← ERROR CODE.
;REGISTERS USED:
;
; R0,R1,SG PASSES ARGUMENTS AND ARE ALTERED
GETPRG: MOV #PROG,R1 ;LOOK FOR A PROGRAM NAME
BR SEEKNM
GETTRN: MOV #TRANS,R1 ;LOOK FOR A TRANSFORM NAME
SEEKNM: MOV R3,-(SP)
MOV R2,-(SP)
MOV R1,-(SP)
MOV #VARTAB,R0 ;LOOK IN VARIABLE HASH TABLE
JSR PC,GETSYM ;DECODE THE SYMBOL
BCC GTTNX ;ALL DONE IF FOUND DEFINED SYMBOL BLK
MOV R1,R3 ;CHECK ERROR CODE
BPL GTTNX ;EXIT IF SYNTAX ERROR OR NO NAME
MOV R0,R2 ;SAVE PTR TO LAST BLK IN BUCKET
MOV #6,R0 ;GET A F.S. BLK OF 6 WORDS
JSR PC,GETBLK
BCS GTTNX ;EXIT IF NO F.S. LEFT
MOV R0,(R2) ;ADD SYMBOL TO HASH TABLE LIST
MOV R0,R1 ;INITIALIZE SYMBOL BLOCK
TST (R1)+
MOV (SP),(R1)+
MOV R3,R2 ;GET NUMBER OF CHARACTERS IN NAME
NEG R3
MOVB (SG)+,(R1)+ ;SAVE SYMBOLIC NAME
SOB R3,.-2
ADD #6,R2 ;NUMBER OF SPACES TO FILL
BEQ GOTNME
MOVB #40,(R1)+ ;FILL SPACES
SOB R2,.-4
GOTNME: CLC
GTTNX: MOV (SP)+,R2 ;DONT NEED TYPE ANY MORE
MOV (SP)+,R2
MOV (SP)+,R3
RTS PC
;END OF "GETTRN" & "GETPRG"
;"GETSTR" - SUBR. TO SAVE STRING POINTER AND ADVANCE SG REGISTER
;THE STRING POINTER IS SAVED IN R0 AND THE POINTER IN THE SG
;REGISTER IS ADVANCED TO THE END OF STRING CHARACTER. A
;SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STRING,SG ;POINT TO INPUT STRING
; JSR PC,GETSTR
;
;THIS ROUTINE NEVER RETURNS A ERROR CODE.
;REGISTERS USED:
;
; R0,SG PASSES ARGUMENTS AND ARE ALTERED
GETSTR: CMPB #40,(SG)+ ;IGNOR LEADING SPACE CHAR.
BEQ GETSTR
DEC SG
MOV SG,R0 ;SAVE STRING POINTER
CMPB #15,(SG)+ ;ADVANCE TO END OF LINE
BNE .-4
DEC SG ;LEAVE IT POINTING AT A C/R
RTS PC
;END OF "GETSTR"
;"GETBLK" - FREE STORAGE ALLOCATOR
;RETURNS A BLOCK OF FREE STORAGE AREA EQUAL IN SIZE TO THE NUMBER OF
;WORDS REQUESTED. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BLKSIZ,R0 ;NUMBER OF WORDS NEEDED
; JSR PC,GETBLK
; BCS ERROR ;NO FREE STORAGE LEFT
;
;ON EXITING, THIS ROUTINE LEAVES A POINTER TO THE START OF THE FREE
;STORAGE AREA IN R0. THIS IS A PTR TO THE FIRST WORD THAT CAN BE
;USED BY THE CALLER, NOT A PTR TO THE BOUNDARY TAG INFORMATION.
GETBLK: MOV R2,-(SP)
ASL R0 ;CONVERT FROM WORD TO BYTE COUNT
CMP (R0)+,(R0)+ ;+ 4 BYTES FOR BOUNDARY TAGS
MOV @#FSPTR,R1 ;PTR TO FIRST FREE BLOCK
BNE FRTRY ;INITIALIZE?
;INITIALIZE FREE STORAGE AREA
MOV #FREEST,R1 ;MARK AREA ABOVE AND BELOW F.S. BUSY
MOV #-1,(R1)+
MOV #HICORE,R2
MOV #-1,(R2)
MOV R1,@#FSPTR ;MAKE WHOLE AREA INTO ONE LARGE BLOCK
SUB R1,R2 ;LENGTH OF LARGE BLOCK
MOV R2,(R1) ;LOWER BOUNDARY TAG
MOV R2,@#HICORE-2 ;UPPER BOUNDARY TAG
;GET THE REQUIRED SPACE
FRTRY: CMP R1,#HICORE-2 ;OFF END OF FREE STORAGE?
BLOS FR2 ;NO
MOV #FREEST,R1 ;YES, RESET PTR TO BEGINNING.
FR2: TST (R1) ;IS THIS AREA BUSY?
BLE FRNEG ;YES
CMP (R1),R0 ;ENOUGH ROOM HERE?
BGE FFOUND ;YES
ADD (R1),R1 ;ON TO NEXT, LOC[LTAG[NEXT]
BR FR1
FRNEG: SUB (R1),R1 ;LOC[LTAG[NEXT]
FR1: CMP R1,@#FSPTR ;CYCLED THROUGH ALL FREE STORAGE?
BNE FRTRY ;NO, TRY AGAIN
MOV #NOFRES,R1 ;RAN OUT OF ROOM, SIGNAL ERROR
JSR PC,TYPERR
SEC
BR GETBDN
FFOUND: BEQ FEXACT ;IF 0 THEN EXACT FIT
MOV R1,R2 ;DIVID BLOCK INTO FOUND AND HOLE
ADD R0,R2 ;LOC[LTAG[HOLE]]
NEG R0 ;BUSY COUNT OF FOUND.
MOV R0,-2(R2) ;RTAG[FOUND] ← NEW FOUND COUNT
MOV R0,-(SP)
ADD (R1),R0 ;LTAG[HOLE] ← NEW HOLE COUNT
MOV R0,(R2)
MOV R2,@#FSPTR ;LOC[LTAG[HOLE]]
MOV R1,R2
TST -(R2)
ADD (R1),R2 ;LOC[RTAG[HOLE]].
MOV R0,(R2) ;RTAG[HOLE] ← NEW HOLE COUNT
MOV (SP)+,(R1)+ ;LTAG[FOUND] ← NEW FOUND COUNT
BR FRRET
FEXACT: MOV R1,R2
ADD (R1),R2 ;LOC[RTAG[FOUND]]
NEG (R1)+ ;SET BOUNDARY TAGS TO BUSY
NEG -(R2)
FRRET: MOV R1,R0 ;LOC[LTAG[FOUND]] + 1.
MOV -2(R0),R2
NEG R2 ;LENGTH COUNT IN WORDS
ASR R2
SUB #2,R2
CLR (R1)+ ;CLEAR THE BLOCK
SOB R2,.-2
GETBDN: MOV (SP)+,R2
RTS PC
;END OF "GETBLK"
;"RELBLK" - RETURNS FREE STORAGE BLOCK
;THIS IS CALLED TO RELEASE A BLOCK OF FREE STORAGE AREA FROM USE. A
;SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #BLOCK,R0 ;PTR TO BLOCK TO BE RELEASED
; JSR PC,GETBLK
;
;NO ERROR MESSAGE IS RETURNED BY THIS ROUTINE
;REGISTERS USED:
; R0 PASSES ARGUMENTS AND R0 AND R1 ARE GARBAGED
RELBLK: TST -(R0) ;LTAG[BLOCK]
MOV R0,R1 ;LOC[LTAG[BLOCK]]
SUB (R0),R0 ;LOC[LTAG[HIGH]]
NEG (R1) ;SIGNAL NOT BUSY
TST -2(R1) ;IS LOW AVAILABLE?
BLT MERGR ;NO, CANNOT MERGE
ADD -2(R1),(R1) ;YES, LTAG[BLOCK] ← NEW COUNT
MOV (R1),-2(R0) ;RTAG[BLOCK] ← NEW COUNT
MOV R0,R1
SUB -2(R1),R1 ;R1 ← LOC[LTAG[LOW]]
MOV -2(R0),(R1) ;LTAG[LOW] ← NEW COUNT
MERGR: TST (R0) ;IS HIGH AVAILABLE?
BLT RLRET ;NO
ADD (R0),(R1) ;LTAG[BLOCK] ← NEW COUNT
CMP @#FSPTR,R0 ;WILL FSPTR POINT INTO VACUUM?
BNE RL1 ;NO
MOV R1,@#FSPTR ;YES, RESET FSPTR ← LOC[LTAG[BLOCK]]
RL1: ADD (R0),R0 ;R0 ← LOC[RTAG[HIGH]] + 2
RLRET: MOV (R1),-2(R0) ;RTAG[BLOCK] ← NEW COUNT
RTS PC
;END OF "RELBLK"
;"TYPERR" - TYPES OUT ERROR MESSAGES
;THE ERROR CODE MUST BE LOADED INTO R1 BEFORE ENTERING THIS
;ROUTINE. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #ERRCODE,R1
; JSR PC,TYPERR
;REGISTERS USED:
; R1 PASSES ARGUMENTS AND R1 & SG ARE ALTERED
TYPERR: MOV R0,-(SP)
BIT #NOSOL,R1 ;SPECIAL CASE OF NO SOLUTION?
BEQ NOTSOL ;NO
MOV #MNOSOL,SG ;YES, TYPE NO SOL. ERROR MES.
JSR PC,TYPSTR
CLR R0 ;FORM ERROR CODE IN HERE
BIC #NOSOL,R1 ;GET JOINT NUMBER
BEQ TYPNUM ;ERROR CODE = 0?
NOSOLL: INC R0
ASR R1
BCC NOSOLL
BR TYPNUM ;TYPE OUT ERROR CODE
NOTSOL: MOV ERRMES(R1),SG ;PUT UP ERROR MESSAGE
CMP #UHALT,R1 ;USER HALT INSTRUCTION?
BNE TYPEDN ;NO
JSR PC,TYPSTR ;YES, TYPE 1ST PART OF MES
TYPNUM: MOV #OUTBUF,SG ;TYPE ASCII NUMBER
JSR PC,PRTINT
MOV #OUTBUF,SG ;NOW TYPE IT
TYPEDN: JSR PC,LINOUT
MOV (SP)+,R0
RTS PC
;END OF "TYPERR"
;ERROR CODE BITS
FINI =0 ;USER PROGRAM COMPLETED
UNKFUN=2 ;UNKNOWN FUNCTION NAME SPECIFIED
BIGSYM=4 ;MORE THAN 6 CHARACTERS USED IN SYMBOL NAME
NOFRES=6 ;FREE STORAGE EXHAUSTED
NOARGU=10 ;NO ARGUMENT FOUND
NOCOMA=12 ;STRANGE CHARACTER BEFORE COMMA
BADNUM=14 ;INVALID NUMBER DECODED
ADCERR=16 ;ADC NOT WORKING
NOPROG=20 ;NO PROGRAM NAME SPECIFIED
BADSTP=22 ;INVALID PROGRAM STEP NUMBER
NULPRG=24 ;EMPTY PROGRAM, NO STEPS
TICLER=26 ;SOMEONE TICKLED THE TTY
NOTDAT=30 ;NO TRANSFORMATION DATA
PANBUT=32 ;PANIC BUTTON HIT
NOHDWR=34 ;HARDWARE SERVO NOT ENABLED
NOTIME=36 ;FUNCTION TOOK TOO LONG TO EXECUTE
RUNERR=40 ;RUNSUB TOOK TOO LONG TO EXECUTE
BADCLS=42 ;HAND CLOSED TO FAR
BADJTN=44 ;ILLEGAL JOINT NUMBER SPECIFIED
OUTRNG=46 ;POSITION OUT OF RANGE
GOODBY=50 ;EXITING TO ODT
UHALT =52 ;USER PROGRAM HALTED
CNTPRO=54 ;CANT PROCEED
NOSOL =200 ;NO VALID ARM SOLUTION
;OUTPUT STRINGS FOR ERROR CODES
ERRMES: .WORD MFINI , MUNKFU, MBIGSY, MNOFRE, MNOARG, MNOCOM
.WORD MBADNU, MADCER, MNOPRO, MBADST, MNULPR, MTICLE
.WORD MNOTDA, MPANBU, MNOHDW, MNOTIM, MRUNER, MBADCL
.WORD MBADJT, MOUTRN, MGOODB, MUHALT, MCNTPR
MFINI: .ASCIZ /USER PROGRAM COMPLETED/
MNOARG: .ASCIZ /**NO ARGUMENT FOUND WHEN EXPECTED**/
MUNKFU: .ASCIZ /**UNDEFINED FUNCTION SPECIFIED**/
MBIGSY: .ASCIZ /**MORE THAN 6 CHARACTERS USED IN SYMBOL NAME**/
MNOFRE: .ASCIZ /**FREE STORAGE EXHAUSTED**/
MNOCOM: .ASCIZ /**UNEXPECTED CHARACTER BEFORE COMMA**/
MBADNU: .ASCIZ /**INVALID NUMBER ENCOUNTERED**/
MADCER: .ASCIZ /**ANALOG TO DIGITAL CONVERTED NOT WORKING**/
MNOPRO: .ASCIZ /**NO PROGRAM NAME SPECIFIED**/
MBADST: .ASCIZ /**INVALID SPECIFICATION OF PROGRAM STEPS**/
MNULPR: .ASCIZ /**NO PROGRAM STEPS DEFINED**/
MNOSOL: .ASCIZ /**REQUIRED ARM SOLUTION DOES NOT EXIST**, CODE=/
MNOTDA: .ASCIZ /**TRANSFORM POSITION NOT YET DEFINED**/
MPANBU: .ASCIZ /**SOMEONE HIT THE PANIC BUTTON**/
MNOHDW: .ASCIZ /**HARDWARE SERVO NOT ENABLED**/
MNOTIM: .ASCIZ /**FUNCTION TOOK TOO LONG TO EXECUTE**/
MRUNER: .ASCIZ /**RUN-TIME FUNCTION CLOCK OVER RUN**/
MBADCL: .ASCIZ /**HAND CLOSED TOO FAR**/
MBADJT: .ASCIZ /**ILLEGAL JOINT NUMBER SPECIFIED**/
MOUTRN: .ASCIZ /**REQUIRED POSITION OUT OF RANGE**/
MGOODB: .ASCIZ /EXITING TO ODT!/
MUHALT: .ASCIZ /PROGRAM HALTED AT STEP /
MCNTPR: .ASCII /**CAN'T PROCEED FROM THIS POINT, USE /
.ASCIZ /"EXEC" INSTRUCTION**/
MTICLE: .ASCIZ /**SOMEONE TICKLED THE TTY CONSOLE**/
.EVEN
;"PACNME" - SUBR. TO PACK A SYMBOLIC NAME INTO A OUTPUT BUFFER
;THE SYMBOL DATA BLOCK ADDRESS FOR THE SYMBOL TO BE PACKED
;MUST BE LOADED INTO R0. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #SYMBLK,R0
; JSR PC,PACNME
;
;NO ERROR MESSAGE IS RETURNED BY THIS ROUTINE.
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,SG ARE GARBAGED
PACNME: MOV R0,-(SP)
ADD #SYMNME,R0 ;GET ADDRESS OF CHARACTERS
MOV #6,R1 ;SIX CHARACTERS
PACNM1: MOVB (R0)+,(SG)+ ;PACK AWAY THAT NAME
SOB R1,PACNM1
MOVB #40,(SG)+ ;PUT IN A SPACE AND NULL CHARACTER
CLRB (SG)
MOV (SP)+,R0
RTS PC
;END OF "PACNME"
;"PTRTRN" - SUBR. TO PRINT A TRANSFORMS NAME AND DATA
;THE TRANS' SYMBOL DATA BLOCK ADDRESS MUST BE LOADED INTO R0. A
;SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #TRNSYM,R0 ;LOAD TRANSFORM ADDRESS
; MOV #TFFLAG,R1 ;1 IF "TF" LISTING,ELSE 0
; JSR PC,PTRTRN
;
;AFTER EXECUTION OF PTRTRN, THE COMPUTED EULER ANGLES ARE LEFT IN
;THE ARRAY "EANGLE". THERE IS NO ERROR MESSAGE RETURNED.
;REGISTERS USED:
;
; R0,R1 PASS ARGUMENTS AND R1 IS MODIFIED
; SG ARE GARBAGED
PTRTRN: MOV R0,-(SP)
MOV #OUTBUF,SG ;PACK THE TRANS NAME IN HERE
MOV R1,-(SP)
BEQ NOTTF ;TF LISTING?
MOV #43124,(SG)+ ;YES, PACK "TF"
MOVB #40,(SG)+
NOTTF: JSR PC,PACNME
TST (SP)+ ;NEED A COMMA IF "TF"
BEQ NOTTF2
MOVB #54,(SG)+
CLRB (SG)
NOTTF2: MOV #OUTBUF,SG ;TYPE THE NAME
JSR PC,TYPSTR
MOV TRNPTR(R0),R0 ;GET PTR TO TRANS DATA
BNE GOTDAT
MOV #PTRMES,SG ;SAY NOT DEFINED IF NO DATA
JSR PC,LINOUT
BR .+6
GOTDAT: JSR PC,PTRANS ;PRINT X,Y,Z,O,A,T
MOV (SP)+,R0
RTS PC
PTRMES: .ASCII /TRANSFORMATION DATA NOT YET DEFINED/
.BYTE 0
.EVEN
;END OF "PTRTRN"
;"PTRANS" - SUBR. TO PRINT A TRANSFORMS X,Y,Z,O,A,T
;THE TRANS DATA ADDRESS MUST BE LOADED INTO R0. A SAMPLE CALLING
;SEQUENCE FOLLOWS:
;
; MOV #TRANS,R0 ;LOAD TRANSFORM ADDRESS
; JSR PC,PTRANS
;
;AFTER EXECUTION OF PTRANS, THE COMPUTED EULER ANGLES ARE LEFT IN
;THE ARRAY "EANGLE". THERE IS NO ERROR MESSAGE RETURNED.
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,SG ARE GARBAGED
PTRANS: MOV R0,-(SP) ;SAVE TRANSFORM POINTER
MOV R2,-(SP)
MOV R3,-(SP)
MOV #EANGLE,R1 ;CONVERT TRANS TO EULER ANGLES
JSR PC,EULER
MOV #OUTBUF,SG ;POINT TO START OF OUTPUT STRING
MOV #EANGLE,R2
MOV #3,R3 ;SET LOOP COUNT TO OUTPUT X,Y,Z
PTRAN1: MOV (R2)+,R0 ;CONVERT DISTANCE TO ASC
JSR PC,PRTDIS
JSR PC,PRTCMA
SOB R3,PTRAN1
MOV #3,R3 ;SET LOOP COUNT TO OUTPUT O,A,T
PTRAN2: MOV (R2)+,R0 ;CONVERT ANGLES TO ASC
JSR PC,PRTANG
JSR PC,PRTCMA
SOB R3,PTRAN2
SUB #2,SG ;PUT IN A NULL CHARACTER
CLRB (SG)
MOV #OUTBUF,SG ;OUTPUT THE STRING
JSR PC,LINOUT
MOV (SP)+,R3 ;RESTORE REGISTERS
MOV (SP)+,R2
MOV (SP)+,R0
RTS PC
HTRANS: .ASCII / X Y Z O/
.ASCII / A T/
.BYTE 0
.EVEN
;END OF "PTRANS"
;"PSTEP" - SUBR. TO PRINT MOTION INSTRUCTION OUT ON TTY
;A POINTER TO THE DATA BLOCK CONTAINING THE MOTION INSTRUCTION MUST
;BE LOADED INTO R1 AND THE STEP NUMBER MUST BE SET IN R0. IF THE
;DATA BLOCK POINTER IS NON-ZERO, THE MOTION INSTRUCTION IS CONVERTED
;TO ASC ALONG WITH ITS STEP NUMBER AND THEY ARE TYPED OUT.
;OTHERWISE, NO TYPE OUT OCCURS. A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #STEPNUM,R0
; MOV #BLKPTR,R1
; JSR PC,PSTEP
;
;AT THE END OF EXECUTION, "OUTBUF" IS ALWAYS LEFT WITH AT LEAST
;THE STEP NUMBER CODED IN ASC. THERE IS NO ERROR MESSAGE
;RETURNED FROM THIS ROUTINE.
;REGISTERS USED:
;
; R0,R1 PASS ARGUMENTS AND R0 IS ALTERED
; SG IS GARBAGED
PSTEP: MOV R4,-(SP)
MOV R3,-(SP)
MOV R2,-(SP)
MOV R1,-(SP) ;SAVE STEP POINTER
MOV #OUTBUF,SG ;CONSTRUCT ASC STRING IN HERE
JSR PC,PRTINT ;STEP NUMBER
MOVB #40,(SG)+ ;SPACE CHARACTER
MOV (SP),R4 ;ALL DONE IF NO INSTRUCTION
BEQ PSTDNE
TST (R4)+
MOV (R4)+,R0 ;MOTION FUNCTION SYMBOL BLOCK
JSR PC,PACNME ;NAME TO ASC
MOV FUNARG+2(R0),R2 ;SPECIFICATIONS OF ARGUMENTS
MOV FUNARG(R0),R3
BEQ PSPTYP ;GO TYPE NAME IF NO ARGS
CMP #STRING,R3 ;SPECIAL CASE OF 1 STRING ARG
BNE PACARG
MOVB (R4)+,(SG)+ ;PACK AWAY STRING ARGUMENT
BNE .-2
BR PSPTYP
PRTARG: BIC #170000,R2 ;DONT WANT SIGN BIT EXTENDED
PACARG: MOV R3,R1 ;NEXT ARGUMENT TYPE
BIC #177761,R1
MOV (R4)+,R0 ;NEXT ARGUMENT
JSR PC,@PRTTAB(R1) ;CONVERT TO ASC
JSR PC,PRTCMA ;COMMA
ASHC #-4,R2 ;REPEAT FOR ALL ARGUMENTS
BNE PRTARG
CLRB -2(SG)
PSPTYP: MOV #OUTBUF,SG ;TYPE THE MOTION COMMAND
JSR PC,LINOUT
PSTDNE: MOV (SP)+,R1
MOV (SP)+,R2
MOV (SP)+,R3
MOV (SP)+,R4
RTS PC
;END OF "PSTEP"
;"MODTRN" - SUBR. TO PERMIT MODIFICATION OF EXISTING TRANSFORMS
;THIS SUBROUTINE IS CALLED TO ALLOW THE USER TO EDIT EXISTING
;TRANSFORMS. THE ONLY REQUIRED ARGUMENT TO THIS ROUTINE IS A TRANS
;POINTER LOADED INTO REGISTER R0. EDITING IS CONTINUED INDEFINITLY
;UNTIL THE USER RESPONSES TO THE QUERY "CHANGES" WITH A NULL LINE
;(I.E. NO REQUESTED CHANGES ). A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV #TRANS,R0
; JSR PC,MODTRN
;
;THERE IS NO ERROR RETURN FROM THIS ROUTINE
;REGISTERS USED:
;
; R0 PASSES ARGUMENT AND IS NOT MODIFIED
; R1,R2,R3,R4,SG ARE GARBAGED
MODTRN: MOV R0,-(SP)
MOV #HTRANS+7,SG ;TYPE OUT THE COLUMN HEADER
JSR PC,LINOUT
BR MODT1
CHGTRN: MOV #EANGLE,R1 ;CONVERT EULER ANGLES BACK TO TRANS
MOV (SP),R0
JSR PC,UNEUL
MODT1: MOV (SP),R0
JSR PC,PTRANS ;TYPE OUT THIS TRANSFORM
MOV #CHGMES,SG ;ASK FOR CHANGES
JSR PC,LINOUT
MOV #INBUF,SG ;READ IN THE CHANGES
JSR PC,INSTR
MOV #EANGLE,R4 ;EULER ANGLES ARE STORED IN HERE
CLR -(SP) ;KEEP TRACK OF ANY CHANGES
MOV #GETDIS,R2 ;READ IN THE THREE DISTANCES
MODT2: MOV #3,R3 ;SET LOOP COUNTER
MODT3: JSR PC,(R2)
BCC ISCORR ;BRANCH IF A CORRECTION WAS TYPED IN
TST R1 ;BRANCH IF ERROR ON INPUT
BNE MODERR
TST (R4)+ ;NO CHANGE MADE
BR NOCORR
ISCORR: MOV R0,(R4)+ ;CHANGE EULER ANGLE
INC (SP) ;INDICATE CHANGE MADE
NOCORR: JSR PC,CLRCMA ;SKIP OVER COMMA
BCC MODCOM ;BRANCH IF NO ERROR
MODERR: JSR PC,TYPERR ;TYPE INPUT ERROR MESSAGE
TST (SP)+
BR MODT1
MODCOM: SOB R3,MODT3 ;REPEAT FOR ALL NUMBERS
CMP #GETANG,R2 ;REPEAT FOR 3 ANGLES
BEQ MODT4
MOV #GETANG,R2
BR MODT2
MODT4: TST (SP)+ ;REPEAT IF CORRECTIONS MADE
BNE CHGTRN
MOV (SP)+,R0
RTS PC
CHGMES: .ASCII /CHANGE?/
.BYTE 0
.EVEN
;END OF "MODTRN"
;"EVAL" - EVALUATES A 5TH ORDER POLYNOMIAL
;"EVAL" COMPUTES THE DESIRED PERCENTAGE CHANGE IN SET POINTS BASED
;UPON THE FRACTION OF TIME THAT HAS ELAPSED SINCE THE START OF A
;MOTION. IF "PTIME" IS THE TIME FOR WHICH THE SET POINT EVALUATION
;IS DESIRED AND "TTIME" IS THE TOTAL MOTION TIME, THE DESIRED
;PERCENTAGE CHANGE IN SET POINT WILL BE:
;
; % CHANGE = 6.0*T↑5 -15*T↑4 +6*T↑3 -1
; WHERE T = PTIME/TTIME
;
;A SAMPLE CALLING SEQUENCE FOLLOWS:
;
; MOV PTIME,R0
; MOV #JTARAY,R1
; MOV TTIME,R2
; JSR PC,EVAL
;
;THE PERCENTAGE CHANGE IS RETURNED IN R0 WHERE 1.0 = '40000. IF PTIME
;IS GREATER THAN OR EQUAL TO TTIME, R0 IS SET TO ZERO AND THE
;"FINAL" FLAG BIT IS SET IN THE 8TH WORD OF THE "JTARAY" ARRAY.
;REGISTERS USED:
; R0,R2 PASS ARGUMENTS AND ARE ALTERED
; R1,R3 ARE GARBAGED
EVAL: CMP R2,R0 ;PAST END OF TRAJECTORY?
BLE TRJEND ;YES
CLR R1 ;% TIME = (PTIME/TTIME)
ASHC #-1,R0
DIV R2,R0
TST R1 ;ROUND
BPL .+4
INC R0
MOV #30000,R2 ;6.0 x T
MUL R0,R2
ASHC #1,R2
TST R3
BPL .+4
INC R2
SUB #74000,R2 ;- 15.0
MUL R0,R2 ;x T
ASHC #1,R2
TST R3
BPL .+4
INC R2
ADD #50000,R2 ;+ 10.0
MOV #3,R1 ;x T**3
TCUBE: MUL R0,R2
ASHC #2,R2
TST R3
BPL .+4
INC R2
SOB R1,TCUBE
MOV R2,R0
SUB #40000,R0 ;-1.0
BR EVALDN
TRJEND: CLR R0 ;USE FINAL SET POINT
BIS #FINAL,16(R1) ;SET POINT EVALUATION DONE
EVALDN: RTS PC
;END OF "EVAL"
;"TIMER" - COMPUTE TOTAL MOTION TIME
;DETERMINES THE TOTAL TIME REQUIRED FOR AN ARM MOTION BY COMPUTING
;THE INDIVIDUAL TIMES REQUIRED BY EACH JOINT AND TAKING THE LARGEST.
;A SAMPLE CALLING SEQUENCE TO THIS ROUTINE FOLLOWS:
;
; MOV #CHANGE,R0
; JSR PC,TIMER
; MOV R0,TIME
;
;THE ONLY ARGUMENT TO THIS ROUTINE IS A POINTER TO A TABLE CONTAINING
;THE CHANGE IN THE JOINT ANGLES FOR THE DESIRED MOTION.
;REGISTERS USED:
; R0 PASSES ARGUMENTS AND IS ALTERED
; R1,R2,R3,R4 ARE GARBAGED
TIMER: MOV R5,-(SP)
MOV R0,R5
MOV #SPEEDS,R1 ;TABLE OF MAXIMUM JOINT SPEEDS
MOV #6,R4 ;SIX JOINTS TO TIME
CLR R0 ;MAXIMUM TRAVERSE TIME
SPDLP: MOV (R5)+,R2 ;COMPUTE JT TRAVERSE TIME
BGE .+4
NEG R2
MUL (R1)+,R2
TST R3 ;ROUND UP
BPL .+4
INC R2
CMP R2,R0 ;KEEP MAXIMUM TIME
BLE .+4
MOV R2,R0
SOB R4,SPDLP
TST R0 ;TIME = 0?
BEQ ZEROT
ADD @#EXTIME,R0 ;ADD A LITTLE TIME FOR SHORT MOVES
BVC .+6
MOV #77777,R0 ;SET TO MAX IF OVERFLOW
ZEROT: TST @#NSPEED ;USER REQUESTED CHANGED?
BEQ TMEDNE ;NO
MUL @#NSPEED,R0 ;YES, CORRECT
CLR @#NSPEED ;ONLY USE ONCE
ASHC #-9.,R0 ;NORMALIZE
TST R0 ;SET TO MAX IF OVERFLOW
BNE MAXTME
MOV R1,R0
BPL .+6
MAXTME: MOV #77777,R0 ;MAXIMUM PERMITTED TIME
TMEDNE: MOV (SP)+,R5
RTS PC
;END OF "TIMER"